function myDWA(option,data,path,sumD,newObstacle,newObstacle_R,newObstacle_v,newObstacle_F,newObstacle_F_R,map,str)
mapSize=data.mapSize;
mysize=50;
mycolor=[0,0,0];
%disp('Dynamic Window Approach sample program start!!')

MapSize=size(map);

obstacleR=0.5*sqrt(2);% 冲突判定用的障碍物半径

tic;
% movcount=0;
% Main loop
figure
plot3(0,0,0);
[p1,p2]=find(map==1);
for ii=1:length(p1)
    xi=p1(ii);
    yi=p2(ii);
    h=2;
    [V,F] = DrawCuboid(1, 1, h, xi,yi,0,[0,0,0]);
    %rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','k')
end
% [p1,p2]=find(map==0);
% for ii=1:length(p1)
%     xi=p1(ii);
%     yi=p2(ii);
%     rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','w')
% end
hold on
no1=[];
no2=[];

plot3(path(:,1),path(:,2),path(:,1)*0+1,'-r','LineWidth',2)
if ~isempty(newObstacle)
    plot3(newObstacle(:,1),newObstacle(:,2),newObstacle(:,1)*0+1,'sr', 'MarkerFaceColor','m','MarkerEdgeColor','m');
end
grid on;
set(gca,'YDir','reverse')	% 翻转Y轴
axis([-1,mapSize(1),-1,mapSize(2),-1,6])
axis equal
figure
plot3(0,0,0);
hold on
[p1,p2]=find(map==1);
for ii=1:length(p1)
    xi=p1(ii);
    yi=p2(ii);
    h=4;
    [V,F] = DrawCuboid(1, 1, h, xi,yi,0,[0,0,0]);

    %rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','k')
end

preT=5; %预测十个步长
[p1,p2]=find(map==1);
obstacle=[p1,p2];
path1=path;
nowP=path(1,:);
path(1,:)=[];
aimP=path(1,:);
v=aimP-nowP;
maxTheta=45; %单位时刻最大转向角度为30度
speed=1; %单位时刻速度
newPath=nowP;
path_M=[];
for i=1:2000
    % 更新动态障碍物位置
    if i<1000
        newObstacle=newObstacle+newObstacle_v;
        path_M=[path_M;newObstacle];
    end
    direction=getDirection(5);
    index=[];
    for j=1:length(direction(:,1))
        v0=direction(j,:);
        theta=acosd(dot(v0,v)/norm(v0)/norm(v));
        if abs(theta)>maxTheta
            index=[index;j];
        end
    end
    direction(index,:)=[];
    D_AimP=zeros(length(direction(:,1)),1)+inf;
    flag=zeros(length(direction(:,1)),1);
    maxT=zeros(length(direction(:,1)),1);
    path_preT=cell(length(direction(:,1)),1);
    for j=1:length(direction(:,1))
        newP=nowP;
        path_preT{j}=[path_preT{j};newP];
        for tt=1:preT
            dir=direction(j,:);
            newP=newP+dir/norm(dir)*speed;
            path_preT{j}=[path_preT{j};newP];
            D1=pdist2([obstacle;],newP);
            if min(D1)<obstacleR
                flag(j)=1;
                break;
            end
            if ~isempty(newObstacle)
                D2=pdist2([newObstacle],newP);


                if min(D2)<newObstacle_R
                    flag(j)=1;
                    break;
                end
            end
            if ~isempty(newObstacle_F)
                D2=pdist2([newObstacle],newP);
                D3=pdist2([newObstacle_F],newP);
                if min(D3)<newObstacle_F_R
                    flag(j)=1;
                    break;
                end
            end
            D_AimP(j)=min(D_AimP(j),pdist2(aimP,newP));
        end
        maxT(j)=tt;
    end
    p=find(maxT>=5);
    if ~isempty(p)
        [~,noDir]=min(D_AimP(p));
        dir=direction(p(noDir),:);
        v=dir;
        nowP=nowP++dir/norm(dir)*speed;
    else
        dir=aimP-nowP;
        dir=dir/norm(dir)*speed;
        nowP=nowP++dir/norm(dir)*0.01;
    end
    newPath=[newPath;nowP];
    % 是否到达目的地
    while norm(nowP-aimP)<5
        if isempty(path)
            %disp('Arrive Goal!!');
            break;
        else
            aimP=path(1,:);
            path(1,:)=[];
        end

    end
    if norm(nowP-aimP)<5
        if isempty(path)
            % disp('Arrive Goal!!');
            break;
        end
    end
    %====Animation====
    % hold off;
    % plot3(0,0,0)
    % hold on;
    ArrowLength=0.5;%
    % 机器人
    if ~isempty(newObstacle)
        xi=newObstacle(1)-newObstacle_R/2;
        yi=newObstacle(2)-newObstacle_R/2;
        h=2;
        [V,F,pc1] = DrawCuboid(newObstacle_R, newObstacle_R, h, xi,yi,0,[1,0,0]);
    end
    %rectangle('Position',[yi-newObstacle_R/2,xi-newObstacle_R/2,newObstacle_R,newObstacle_R],'FaceColor','r')
    if ~isempty(newObstacle_F)
        xi=newObstacle_F(1)-newObstacle_F_R/2;
        yi=newObstacle_F(2)-newObstacle_F_R/2;
        h=2;
        [V,F,pc2] = DrawCuboid(newObstacle_R, newObstacle_R, h, xi,yi,0,[0,1,1]);
    end
    %rectangle('Position',[yi-newObstacle_F_R/2,xi-newObstacle_F_R/2,newObstacle_F_R,newObstacle_F_R],'FaceColor','c')
    %     quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');
    hold on;
    clear h
    for j=1:length(direction(:,1))
        h(j)=plot3(path_preT{j}(:,1),path_preT{j}(:,2),path_preT{j}(:,2)*0+1,'-g','LineWidth',2);
    end

    g(1)=plot3(aimP(1),aimP(2),1,'*r');
    g(2)=plot3(path1(:,1),path1(:,2),path1(:,1)*0+1,'--m','LineWidth',2);
    g(3)=plot3(newPath(:,1),newPath(:,2),newPath(:,1)*0+1,'-b','LineWidth',2);
    if ~isempty(newObstacle_F)
        gg(1)=plot3(path_M(:,1),path_M(:,2),path_M(:,1)*0+1,'-r','LineWidth',2);
    end
    %plot(newObstacle(:,2),newObstacle(:,1),'sr', 'MarkerFaceColor','m','MarkerEdgeColor','m');
    grid on;
    set(gca,'YDir','reverse')	% 翻转Y轴
    axis equal
    axis([-1,mapSize(1),-1,mapSize(2),-1,6])
    title([str,',t=',num2str(i)])
    drawnow;
    delete(h)
    delete(g)
    if ~isempty(newObstacle)
        delete(pc1)
    end
    if ~isempty(newObstacle_F)
        delete(gg)
        delete(pc2)
    end
end
hold off;
plot3(0,0,0)
hold on;
ArrowLength=0.5;%
% 机器人
[p1,p2]=find(map==1);
for ii=1:length(p1)
    xi=p1(ii);
    yi=p2(ii);
    h=4;
    [V,F] = DrawCuboid(1, 1, h, xi,yi,0,[0,0,0]);
    %rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','k')
end
% [p1,p2]=find(map==0);
% for ii=1:length(p1)
%     xi=p1(ii);
%     yi=p2(ii);
%     rectangle('Position',[yi-0.5,xi-0.5,1,1],'FaceColor','w')
% end
if ~isempty(newObstacle)
    xi=newObstacle(1)-newObstacle_R/2;
    yi=newObstacle(2)-newObstacle_R/2;
    h=2;
    [V,F] = DrawCuboid(newObstacle_R, newObstacle_R, h, xi,yi,0,[1,0,0]);
end
%rectangle('Position',[yi-newObstacle_R/2,xi-newObstacle_R/2,newObstacle_R,newObstacle_R],'FaceColor','r')
if ~isempty(newObstacle_F)
    xi=newObstacle_F(1)-newObstacle_F_R/2;
    yi=newObstacle_F(2)-newObstacle_F_R/2;
    h=2;
    [V,F] = DrawCuboid(newObstacle_R, newObstacle_R, h, xi,yi,0,[0,1,1]);
end
%rectangle('Position',[yi-newObstacle_F_R/2,xi-newObstacle_F_R/2,newObstacle_F_R,newObstacle_F_R],'FaceColor','c')
%     quiver(x(1),x(2),ArrowLength*cos(x(3)),ArrowLength*sin(x(3)),'ok');
hold on;

newPath=[newPath;aimP];
plot3(aimP(1),aimP(2),1,'*r');
plot3(path1(:,1),path1(:,2),path1(:,1)*0+1,'--m','LineWidth',2)
plot3(newPath(:,1),newPath(:,2),newPath(:,1)*0+1,'-b','LineWidth',2)
if ~isempty(newObstacle_F)
    plot3(path_M(:,1),path_M(:,2),path_M(:,1)*0+1,'-y','LineWidth',2)
end

%plot(newObstacle(:,2),newObstacle(:,1),'sr', 'MarkerFaceColor','m','MarkerEdgeColor','m');
grid on;
set(gca,'YDir','reverse')	% 翻转Y轴
axis equal
axis([-1,mapSize(1),-1,mapSize(2),-1,6])
title([str,',t=',num2str(i)])
drawnow;
toc;
sumD=0;
for i=1:length(newPath(:,1))-1
    sumD=sumD+norm(newPath(i,:)-newPath(i+1,:));
end
%disp([str,'总距离：',num2str(sumD)])
end
%%
